#!/usr/bin/python  
# coding=utf-8  
#本段代码实现树莓派智能小车的红外避障效果
#代码使用的树莓派GPIO是用的BOARD编码方式。 
import RPi.GPIO as GPIO
#电机控制器
from motor import Motor
#红外距离传感器
from infrared_sensor import InfraredSensor
#超声波避障
from adafruit.Adafruit import Adafruit

import time
import sys
import tensorflow as tf
import numpy as np


#动作常量
ACTION_UP = 0
ACTION_DOWN = 1
ACTION_LEFT = 2
ACTION_RIGHT = 3

#电机控制类
motor = Motor()
#超声波
adafruit = Adafruit()

#加载自动驾驶模型
model = tf.keras.models.load_model(sys.path[0]+'/ai/model.h5')

#采集距离
def collect_dists(adafruit,is_opposite=False,step_angle=0.25):
    dists = [0,0,0,0,0]
    
    #从右边开始采集
    if is_opposite == False:
        for i in range(5):
            dists[i]=adafruit.angel_dist(step_angle*i)
            time.sleep(0.1)
    #从左边开始采集
    else:
        for i in range(4,-1,-1):
            dists[i]=adafruit.angel_dist(step_angle*i)
            time.sleep(0.1)
    return dists
    
def setup():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BCM)       # Numbers GPIOs by physical location

    #初始化电机
    motor.init()
    #初始化超声波
    adafruit.init()

if __name__ == '__main__':
    setup()

    #启动电机
    motor.start()
    
    is_opposite = False
    
    try:
        while True:
            #采集超声波距离
            dists = collect_dists(adafruit,is_opposite)
            #调转采集的方向
            is_opposite = not is_opposite
            
            # 利用模型对测试数据进行预测
            dists = np.array([dists],np.float64)
            dists = dists / 500.0
            y = model.predict(dists)
            print(y)
            #计算执行动作
            action = np.argmax(y)
            
            # 执行动作
            if action == ACTION_UP:
                print ("up")
                motor.up(20,0)
            elif action == ACTION_DOWN:
                print ("down")
                motor.down(30,0.5)
                motor.stop(0.01)
            elif action == ACTION_LEFT:
                print ("left")
                motor.left(30,0.5)
                motor.stop(0.01)
            elif action == ACTION_RIGHT:
                print ("right")
                motor.right(30,0.5)
                motor.stop(0.01)
                
    except KeyboardInterrupt:  # When 'Ctrl+C' is pressed, the child program destroy() will be  executed.
        GPIO.cleanup()
